#include "IMU.h"
#include "MPU6500.h"
#include "delay.h"
#include "Mahony.h"
#include "LSM303.h"


//姿态解算所需数据
typedef struct{
	float gyro[3];
	float acc[3];
	float magnet[3];
	float q[4];
}IMU_Info;


/**
 * @Description IMU硬件初始化 
 */
void IMU_HardwareInit(void)
{
	LSM303_Init();
	delay_ms(100);
	MPU6500_Init();
	delay_ms(100);
	MPU6500_Init_Offset();
}


/**
 * @Description 读取姿态解算所需角速度、加速度、磁场数据(完成单位转换)
 */
void IMU_getVals(IMU_Info *info)
{
	int16_t gyro[3],acc[3],magnetic[3];
	
	MPU6500_readGyro_Acc(&gyro[0],&acc[0]);
	//磁场
	LSM303_readMag(&magnetic[0]); 
	int i = 0;
	for(;i<3;i++)
	{
		//转换为真实值
		info->gyro[i] = ((float) gyro[i]) * 2000 / 32768;
		//加速度 = ADC测量值 × 量程 除以 ADC精度
		info->acc[i] = (float) acc[i] * 2 / 32768;
		info->magnet[i] = (float) magnetic[i] * 4 / 32768;
	}
}

//光流数据融合使用的矩阵
volatile float RDrone_R[3][3];
volatile float RDrone_R2[3][3];
volatile float Accer[3];


/**
* @Description IMU姿态更新
 */
void IMU_Info_Update(void)
{
	
	static IMU_Info info={{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0},{0.0, 0.0, 0.0},{1.0,0.0,0.0,0.0}};
	static float q_yaw[4]={1.0,0.0,0.0,0.0};	//yaw角度使用九轴Mahony算法更新、与六轴使用不同的四元数
	
	static float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
	
	IMU_getVals(&info);
	//Accer用于位置融合
	Accer[0]=info.acc[0];
	Accer[1]=info.acc[1];
	Accer[2]=info.acc[2];
	
	MahonyAHRSupdateIMU(info.gyro[0]*(PI/180),info.gyro[1]*(PI/180),info.gyro[2]*(PI/180),info.acc[0],info.acc[1],info.acc[2],&info.q[0]);

	q0q0 = info.q[0]*info.q[0];
	q0q1 = info.q[0]*info.q[1];
	q0q2 = info.q[0]*info.q[2];
	q0q3 = info.q[0]*info.q[3];
	q1q1 = info.q[1]*info.q[1];
	q1q2 = info.q[1]*info.q[2];
	q1q3 = info.q[1]*info.q[3];
	q2q2 = info.q[2]*info.q[2];
	q2q3 = info.q[2]*info.q[3];
	q3q3 = info.q[3]*info.q[3]; 
	
	//位置融合使用的矩阵
	RDrone_R2[0][0] = 2*(q0q0+q1q1)-1;
	RDrone_R2[0][1] = 2*(q1q2-q0q3);
	RDrone_R2[0][2] = 2*(q1q3+q0q2);
	RDrone_R2[1][0] = 2*(q1q2+q0q3);		
	RDrone_R2[1][1] = 2*(q0q0+q2q2)-1;
	RDrone_R2[1][2] = 2*(q2q3-q0q1);
	RDrone_R2[2][0] = 2*(q1q3-q0q2);
	RDrone_R2[2][1] = 2*(q2q3+q0q1);
	RDrone_R2[2][2] = 2*(q0q0+q3q3)-1;
	
	RDrone_R[0][0] = q0q0 + q1q1 - q2q2 - q3q3;
	RDrone_R[0][1] = 2.f * (q1q2 + q0q3);
	RDrone_R[0][2] = 2.f * (q1q3 - q0q2);
	RDrone_R[1][0] = 2.f * (q1q2 - q0q3);		
	RDrone_R[1][1] = q0q0 - q1q1 + q2q2 - q3q3;
	RDrone_R[1][2] = 2.f * (q2q3 + q0q1);
	RDrone_R[2][0] = 2.f * (q1q3 + q0q2);
	RDrone_R[2][1] = 2.f * (q2q3 - q0q1);
	RDrone_R[2][2] = q0q0 - q1q1 - q2q2 + q3q3;	
	
	
	//为何乘100？
	RT_Info.rateRoll=info.gyro[0]*100; 
	RT_Info.ratePitch=info.gyro[1]*100;
	RT_Info.rateYaw=info.gyro[2]*100;
	
	float *q = &(info.q[0]);
	
	//由四元数矩阵计算姿态角
	RT_Info.Roll=(atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;
	RT_Info.Pitch=-safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;
	
	//使用九轴数据更新yaw角
	MahonyAHRSupdate(info.gyro[0]*(PI/180),info.gyro[1]*(PI/180),info.gyro[2]*(PI/180),info.acc[0],info.acc[1],info.acc[2],info.magnet[0],info.magnet[1],info.magnet[2],q_yaw);
	RT_Info.Yaw = -atan2(2 * q_yaw[1] * q_yaw[2] + 2 * q_yaw[0] * q_yaw[3], -2 * q_yaw[2]*q_yaw[2] - 2 * q_yaw[3] * q_yaw[3] + 1)* 180/PI;
	
}

